#include "AxisCamera.h" 
#include "BaeUtilities.h"
#include "FrcError.h"
#include "PCVideoServer.h"
#include "PIDTurret.h"
#include "TrackAPI.h" 
#include "WPILib.h"

// Auto Hood Parameters
#define POINT_BLANK_SIZE 6.52f
#define MAX_RANGE_SIZE 0.75f

// Turret Pot Values (competition)
//#define MIN_TURRET_POT 1.52f
//#define MAX_TURRET_POT 3.47f
//#define HOPPER_TURRET_POT 2.70f

// Turret Pot Values (practice 'bot)
#define MIN_TURRET_POT 1.93f //3.33f
#define MAX_TURRET_POT 3.06f //4.07f
#define HOPPER_TURRET_POT 2.80f //3.87f

// Hood Pot Values
#define MIN_HOOD_POT 1.78f
#define MAX_HOOD_POT 3.45f
#define TRAILER_HOOD_POT 2.45f

// Compitetion Bot
//#define MAX_TRAPDOOR_POT 3.67f
//#define MIN_TRAPDOOR_POT 2.18f

// Practice Bot
#define MAX_TRAPDOOR_POT 2.43f
#define MIN_TRAPDOOR_POT 1.09f

// Size thresholds for smallest colour patch to bother processing
#define MIN_PARTICLE_TO_IMAGE_PERCENT 0.01f
#define MAX_PARTICLE_TO_IMAGE_PERCENT 100.0f

// Drive pulsing parameters
#define DRIVE_PULSE_PERIOD 0.1f // Pulse Period in seconds
#define DRIVE_PULSE_ON 0.075f // Pulse on duration in seconds

// Shooter pulsing
#define SHOOTER_PULSE_PERIOD 0.1f // Pulse period - 10 milli-seconds
#define SHOOTER_PULSE_ON 0.025f // Pulse on for - 7.5 milli-seconds

class RobotDemo : public SimpleRobot
{
	// Joysticks: 1 = Driver, 4 = Operator
	Joystick stick1, stick2, stick3, stick4;

	// Various Function Motors
	Victor trapdoor_open;
	Victor pickup1;
	Victor pickup2;
	Victor hood;
	Victor turret;
	Victor trapdoor_feed;
	Victor right;
	Victor left;
	Victor shooter;

	// Drivetrain Object
	RobotDrive myRobot;

	// Pulse Timer is for pulsing drivetrain
	// Cam Timer is for processing camera frame 
	Timer pulseTimer, camTimer, shooterTimer, atTimer;

	// PID objects for rotating turret left/right
	PIDCameraX pidCameraX;
	PIDTurretX pidTurretX;
	PIDController pidControlTurret;

	// PID objects for adjusting hood angle
	PIDHoodPot pidHoodPot;
	PIDHood pidHood;
	PIDController pidControlHood;

	// PID objects for adjusting shooter speed
	PIDShooterEncoder pidShooterEncoder;
	PIDShooter pidShooter;
	PIDController pidControlShooter;
		
	// PID objects for rotating turret for hopper shot
	PIDTurretPot pidTurretPot;
	PIDTurretPotOut pidTurretPotOut;
	PIDController pidControlTurretPot;
	
	// PID objects for adjusting trap door angle
	PIDTrapDoorPot pidTrapDoorPot;
	PIDTrapDoor pidTrapDoor;
	PIDController pidControlTrapDoor;
	
	// Various Analog Sensors
	Gyro gyro;
	AnalogChannel potHood, potTurret, potTrapdoor;

	// Colour tracking threshold object
	TrackingThreshold tdata;

	// Various drive transfer function variables
	float x1,y1,z1,twist1,x2,y2,y3;
	double hood_amp;
	int auto_sw;
	float auto_pot;

	// Driver statiom instance
	DriverStation *ds;

public:
	RobotDemo(void) :

		// Assign joysticks to specific ports
		stick1(1),
		stick2(2),
		stick3(3),
		stick4(4),

		// Assign PWM outputs to various function motors
		trapdoor_open(1),//switch with feed at competition bot
		pickup1(2),
		pickup2(3),
		hood(4),
		turret(5),
		trapdoor_feed(6),//switch with open for competition robot.
		right(7),
		left(8),
		shooter(9),

		// Setup drivetrain object
		myRobot(left,right),

		// Initialize timers
		pulseTimer(),
		camTimer(),
		shooterTimer(),
		atTimer(),

		// Initialize PID objects for rotating turret left/right
		pidCameraX(),
		pidTurretX(),
		pidControlTurret(0.5f, 0.0f, 0.3f, &pidCameraX, &pidTurretX, 0.05f),

		// Initialize PID objects for adjusting the hood
		pidHoodPot(),
		pidHood(),
		pidControlHood(0.5f, 0.0f, 0.1f, &pidHoodPot, &pidHood, 0.05f),

		// Initialize PID objects for running the shooter
		pidShooterEncoder(),
		pidShooter(),
		pidControlShooter(0.8f, 0.0f, 0.0f, &pidShooterEncoder, &pidShooter, 0.1f),

		// Initialize PID objects for rotating turret based on pot value
		pidTurretPot(),
		pidTurretPotOut(),
		pidControlTurretPot(1.0f, 0.0f, 0.0f, &pidTurretPot, &pidTurretPotOut, 0.05f),

		// Initialize PID objects for trap door
		pidTrapDoorPot(),
		pidTrapDoor(),
		pidControlTrapDoor(0.6f, 0.00f, 0.1f, &pidTrapDoorPot, &pidTrapDoor, 0.05f),

		// Initialize various analog sensors
		gyro(1),
		potHood(3),
		potTurret(4),
		potTrapdoor(5),

		// Initialize colour threshold object
		tdata()
		{

		// Get driver station instance
		ds = DriverStation::GetInstance();

		// Configure PID objects for turret rotation left/right
		pidControlTurret.SetInputRange(-1,1);
		pidControlTurret.SetOutputRange(-0.1,0.1);
		pidControlTurret.SetSetpoint(0);
		pidControlTurret.SetTolerance(0.0);

		// Configure PID objects for hood angle adjustment
		pidControlHood.SetInputRange(MIN_HOOD_POT, MAX_HOOD_POT);
		pidControlHood.SetOutputRange(-0.25f,0.25f);
		pidControlHood.SetSetpoint(0);
		pidControlHood.SetTolerance(0.0);

		// Configure PID objects for shooter speed
		pidControlShooter.SetInputRange(1,10);
		pidControlShooter.SetOutputRange(0,1);
		pidControlShooter.SetSetpoint(0);
		pidControlShooter.SetTolerance(0.0);
		
		// Configure PID objects for turret rotation based on pot value
		pidControlTurretPot.SetInputRange(MIN_TURRET_POT, MAX_TURRET_POT);
		pidControlTurretPot.SetOutputRange(-1,1);
		pidControlTurretPot.SetSetpoint(0);
		pidControlTurretPot.SetTolerance(0.0);
		
		// Configure PID objects for trapdoor rotation based on pot value
		pidControlTrapDoor.SetInputRange(MIN_TRAPDOOR_POT,MAX_TRAPDOOR_POT);
		pidControlTrapDoor.SetOutputRange(-1.0,1.0);
		pidControlTrapDoor.SetSetpoint(MAX_TRAPDOOR_POT);
		pidControlTrapDoor.SetTolerance(0.0);
		
		// Set watchdog
		GetWatchdog().SetExpiration(100);

		// Initialize variables for driver transfer functions
		x1 = 0;
		y1 = 0;
		z1 = 0;
		twist1 = 0;
		x2 = 0;
		y2 = 0;
		y3 = 0;
		auto_sw=0;
		auto_pot=0;

		hood_amp = (MAX_HOOD_POT - TRAILER_HOOD_POT) / ((MAX_RANGE_SIZE - POINT_BLANK_SIZE)*(MAX_RANGE_SIZE - POINT_BLANK_SIZE));
		
		// Don't printf to anywhere but the screen
		SetDebugFlag(DEBUG_SCREEN_ONLY);

		// Start the camera
		if (StartCameraTask(20, 0, k160x120, ROT_0) == -1) {
			dprintf( LOG_ERROR,"Failed to spawn camera task; Error code %s", 
					GetVisionErrorText(GetLastVisionError()) );
		}
		// Print the status of the camera
		printf("Camera Initialization: %s\n",GetVisionErrorText(GetLastVisionError()));

		// TODO: Select colour threshold based on switch
		tdata = GetTrackingData(PINK, FLUORESCENT);
		tdata.luminance.minValue = 90;
		tdata.luminance.maxValue = 140;
		tdata.hue.minValue = 200;
		tdata.hue.maxValue = 255;
		tdata.saturation.minValue = 60;
		tdata.saturation.maxValue = 190;

		// Initialize gyro
		gyro.SetSensitivity(0.007);

		// Initialize encoder
		pidShooterEncoder.shooterEncoder->SetUpSourceEdge(true, true);
		pidShooterEncoder.shooterEncoder->Start();

		}
	~RobotDemo(void)
	{
		// Delete driver station instance on close
		delete ds;
	}
	void Autonomous(void)
	{		
		// Start the camera feed
		//PCVideoServer pc;

		// Feed the watchdog
		GetWatchdog().Feed();

		// Start the camera image processing timer
		camTimer.Reset();
		camTimer.Start();
		
		atTimer.Start();
		atTimer.Reset();
		
		pulseTimer.Start();
		pulseTimer.Reset();
		
		//auto_sw=ds->GetDigitalIn(1);
		//auto_pot=ds->GetAnalogIn(4);
		
		while (IsAutonomous()) 
		{
			// Feed watchdog
			GetWatchdog().Feed();

			// Track turret and hood
			UpdateCamLED(true);
			if(atTimer.Get()<=2.0)
			{
				if(pulseTimer.Get() > DRIVE_PULSE_PERIOD)
				{
					pulseTimer.Reset();
				}
				if(pulseTimer.Get() > DRIVE_PULSE_ON)
				{
					// Pulse off
					myRobot.ArcadeDrive(0,0,false);
				}
				else
				{
					// Pulse on
					myRobot.ArcadeDrive(-1.0, -0.7,false);		
				}

			}
			if(atTimer.Get()<=3.5)
			{
				if(pulseTimer.Get() > DRIVE_PULSE_PERIOD)
				{
					pulseTimer.Reset();
				}
				if(pulseTimer.Get() > DRIVE_PULSE_ON)
				{
					// Pulse off
					myRobot.ArcadeDrive(0,0,false);
				}
				else
				{
					// Pulse on
					myRobot.ArcadeDrive(-1.0, 0.2,false);		
				}
			}
			if(atTimer.Get()<=6.0)
			{
				if(pulseTimer.Get() > DRIVE_PULSE_PERIOD)
				{
					pulseTimer.Reset();
				}
				if(pulseTimer.Get() > DRIVE_PULSE_ON)
				{
					// Pulse off
					myRobot.ArcadeDrive(0,0,false);
				}
				else
				{
					// Pulse on
					myRobot.ArcadeDrive(0.0, 1.0,false);		
				}
			}
			/*if(atTimer.Get()<=2.0)
					{
						if(pulseTimer.Get() > DRIVE_PULSE_PERIOD)
						{
							pulseTimer.Reset();
						}
						if(pulseTimer.Get() > DRIVE_PULSE_ON)
						{
							// Pulse off
							myRobot.ArcadeDrive(0,0,false);
						}
						else
						{
							// Pulse on
							myRobot.ArcadeDrive(-1.0, 0.75,false);		
						}

					}
					if(atTimer.Get()<=3.0)
					{
						if(pulseTimer.Get() > DRIVE_PULSE_PERIOD)
						{
							pulseTimer.Reset();
						}
						if(pulseTimer.Get() > DRIVE_PULSE_ON)
						{
							// Pulse off
							myRobot.ArcadeDrive(0,0,false);
						}
						else
						{
							// Pulse on
							myRobot.ArcadeDrive(-1.0, -0.2,false);		
						}
					}
					if(atTimer.Get()<=5.25)
					{
						if(pulseTimer.Get() > DRIVE_PULSE_PERIOD)
						{
							pulseTimer.Reset();
						}
						if(pulseTimer.Get() > DRIVE_PULSE_ON)
						{
							// Pulse off
							myRobot.ArcadeDrive(0,0,false);
						}
						else
						{
							// Pulse on
							myRobot.ArcadeDrive(0.0, -1.0,false);		
						}
					}
					if(atTimer.Get()<=2.0)
					{
						if(pulseTimer.Get() > DRIVE_PULSE_PERIOD)
						{
							pulseTimer.Reset();
						}
						if(pulseTimer.Get() > DRIVE_PULSE_ON)
						{
							// Pulse off
							myRobot.ArcadeDrive(0,0,false);
						}
						else
						{
							// Pulse on
							myRobot.ArcadeDrive(-1.0, -0.8,false);		
						}

					}
					if(atTimer.Get()<=4.0)
					{
						if(pulseTimer.Get() > DRIVE_PULSE_PERIOD)
						{
							pulseTimer.Reset();
						}
						if(pulseTimer.Get() > DRIVE_PULSE_ON)
						{
							// Pulse off
							myRobot.ArcadeDrive(0,0,false);
						}
						else
						{
							// Pulse on
							myRobot.ArcadeDrive(-1.0, 0.1,false);		
						}
					}
					if(atTimer.Get()<=6.0)
					{
						if(pulseTimer.Get() > DRIVE_PULSE_PERIOD)
						{
							pulseTimer.Reset();
						}
						if(pulseTimer.Get() > DRIVE_PULSE_ON)
						{
							// Pulse off
							myRobot.ArcadeDrive(0,0,false);
						}
						else
						{
							// Pulse on
							myRobot.ArcadeDrive(0.0, 1.0,false);		
						}
					}*/
		}

		// Stop robot
		myRobot.Drive(0.0, 0.0); 

		// Stop turret
		turret.Set(0.0);

		// Stop hood
		hood.Set(0.0);
	}
	void OperatorControl(void)
	{
		static bool trapDoor_direction = false;
		
		// Start the camera feed
		PCVideoServer pc;

		// Set default drivemode to Kaj drive
		char drivemode = 0;

		// Enable watchdog
		GetWatchdog().SetEnabled(true); 

		// Reset pulse timer
		pulseTimer.Reset();
		pulseTimer.Start();

		// Reset camera timer
		camTimer.Reset();
		camTimer.Start();

		// Reset shooter timer
		shooterTimer.Reset();
		shooterTimer.Start();

		while (IsOperatorControl())
		{

			// Feed watchdog
			GetWatchdog().Feed();

			// Update the hood pot reading to the hood PID
			pidHoodPot.hoodPot = potHood.GetVoltage();
			
			// Update the turret pot reading to the turret PID
			pidTurretPot.x = potTurret.GetVoltage();
			
			// Update the trapdoor pot reading to the turret PID
			pidTrapDoorPot.trapDoorPot = potTrapdoor.GetVoltage();
						
			// Call the camera handler - pass status of whether camera should be tracking
			UpdateCamLED(stick4.GetRawButton(6));

			// Read switches to determine which controller drive mode is being used
			drivemode = 0;

			// Check if "Turbo" buttons are being pressed
			if(!(stick1.GetRawButton(6) ||
					stick2.GetTrigger() ||
					stick3.GetTrigger()))
			{
				// If no turbo, pulse drivetrain on and off
				if(pulseTimer.Get() > DRIVE_PULSE_PERIOD)
				{
					pulseTimer.Reset();
				}
				if(pulseTimer.Get() > DRIVE_PULSE_ON)
				{
					// Pulse off
					myRobot.ArcadeDrive(0,0,false);
				}
				else
				{
					// Pulse on
					TeleOpDrive(drivemode);		
				}
			}
			else
			{
				// No pulsing
				TeleOpDrive(drivemode);
			}

			// Check if pick-up buttons are pressed
			if (stick4.GetRawButton(5) || stick1.GetRawButton(8))
			{
				// Feed in
				pickup1.Set(1);
				pickup2.Set(1);
				
				if (trapDoor_direction)
				{
					trapdoor_feed.Set(1);
				}
				else
				{
					trapdoor_feed.Set(-1);
				}
			}
			else if (stick4.GetRawButton(7) || stick1.GetRawButton(7))
			{
				// Feed out
				pickup1.Set(-1);
				pickup2.Set(-1);
				
				if (trapDoor_direction)
				{
					trapdoor_feed.Set(-1);
				}
				else
				{
					trapdoor_feed.Set(1);
				}
			}
			else
			{
				// Feed stop
				pickup1.Set(0);
				pickup2.Set(0);
				trapdoor_feed.Set(0);
			}

			// Check if trap-door buttons are pressed
			if (stick4.GetRawButton(3))
			{
				// Trap door closed
				
				trapDoor_direction = false;
				
				pidControlTrapDoor.SetSetpoint(MAX_TRAPDOOR_POT);
				pidControlTrapDoor.Enable();
				
				// Manual for setting Pot Value
				//trapdoor_open.Set(-0.25);
			}
			else if (stick4.GetRawButton(4) || stick1.GetRawButton(5))
			{
				// Trap door open
				
				trapDoor_direction = true;
				
				pidControlTrapDoor.SetSetpoint(MIN_TRAPDOOR_POT);
				pidControlTrapDoor.Enable();
				
				// Manual for setting Pot Value
				//trapdoor_open.Set(0.25);
			}
			else
			{
				// Manual for setting Pot Value
				//trapdoor_open.Set(0);
			}
			// Comment out for Manual for setting Pot Value
			trapdoor_open.Set(pidTrapDoor.trapDoor);
			
			// Check if shooter button is pressed
			
			if (stick4.GetRawButton(8))
			{
				// SHOOT!
				pidControlShooter.SetSetpoint(9);
				pidControlShooter.Enable();
				shooter.Set(-pidShooter.shooter);

			}			
			else if (stick4.GetRawButton(2))
			{
				pidControlTurretPot.SetSetpoint(HOPPER_TURRET_POT);
				pidControlTurretPot.Enable();
				turret.Set(-pidTurretPotOut.x);

				pidControlHood.SetSetpoint(MIN_HOOD_POT);
				pidControlHood.Enable();
				hood.Set(pidHood.hood);

				pidControlShooter.SetSetpoint(2);
				pidControlShooter.Enable();
				shooter.Set(-pidShooter.shooter);
			}
			else if (stick1.GetRawButton(2))
			{
				// SHOOT!
				pidControlShooter.SetSetpoint(7);
				pidControlShooter.Enable();
				shooter.Set(-pidShooter.shooter);

			}	
			else
			{
				// Run backwards slowly to prevent jam-ups
				pidControlShooter.SetSetpoint(2);
				pidControlShooter.Enable();
				shooter.Set(pidShooter.shooter);
			}
			if (stick4.GetRawButton(1))
			{
				pidControlHood.SetSetpoint(TRAILER_HOOD_POT);
				pidControlHood.Enable();
				hood.Set(pidHood.hood);
				TurretManual();
			}
		}
	}

	void TeleOpDrive(char drivemode)
	{
		// Send the proper commands to drivetrain motors depending on drive mode selected
		x1=stick1.GetX();
		y1=stick1.GetY();
		z1=stick1.GetZ();
		twist1=stick1.GetTwist();
		x2=stick2.GetX();
		y2=stick2.GetY();
		y3=stick3.GetY();

		switch(drivemode)
		{
		case 0:
			// Kaj mode - Gamepad
			myRobot.ArcadeDrive(y1,z1,false);
			break;
		case 1:
			// Tank mode - Gamepad
			myRobot.TankDrive(y1,twist1);
			break;
		case 2:
			// 1-Stick mode - Joystick
			myRobot.ArcadeDrive(y2,x2,false);
			break;
		case 3:
			// Tank mode - Joystick
			myRobot.TankDrive(y2,y3);
			break;
		}
	}

	void TurretManual()
	{
		pidControlTurret.Disable();
		pidControlTurret.Reset();
		
		pidControlTurretPot.Disable();
		pidControlTurretPot.Reset();
								
		if ((potTurret.GetVoltage() > MAX_TURRET_POT) && (stick4.GetZ() < 0))
		{
			turret.Set(0);
		}
		else if ((potTurret.GetVoltage() < MIN_TURRET_POT) && (stick4.GetZ() > 0))
		{
			turret.Set(0);
		}
		else
		{
			turret.Set(stick4.GetZ() * stick4.GetZ() * stick4.GetZ());
		}
	}
	
	void HoodManual()
	{
		pidControlHood.Disable();
		pidControlHood.Reset();
		
		// Run hood depending on operator joystick
		if (stick4.GetY() < 0)
		{
			if (potHood.GetVoltage() < MIN_HOOD_POT)
				hood.Set(0);
			else
				// TODO: Check if this should be /4 on competition robot
				hood.Set((stick4.GetY() * stick4.GetY() * stick4.GetY()) / 2);
		}
		else
		{
			if (potHood.GetVoltage() > MAX_HOOD_POT)
				hood.Set(0);
			else
				hood.Set((stick4.GetY() * stick4.GetY() * stick4.GetY()) / 4);
		}
	}
	
	void UpdateCamLED(bool tracking)
	{
		// Declare variables used to show camera tracking statuses
		static unsigned char camLED = 0;
		static unsigned char RangeLED = 0;

		// Declare particle analysis object to store pink/green locations
		static ParticleAnalysisReport par;

		// Check to see if a new image from the camera is available
		if(camTimer.Get() > 0.05)
		{
			// Reset the camera timer to wait for next frame
			camTimer.Reset();

			// Try and find the colour
			if (FindColor(IMAQ_HSL, &tdata.hue, &tdata.saturation, &tdata.luminance, &par)
					&& par.particleToImagePercent <= MAX_PARTICLE_TO_IMAGE_PERCENT
					&& par.particleToImagePercent >= MIN_PARTICLE_TO_IMAGE_PERCENT)
			{

				// Pass X-coordinate of largest colour particle to turret PID
				pidCameraX.x = par.center_mass_x_normalized;

				// Check if we're supposed to be tracking
				if(tracking)
				{
					if ((potTurret.GetVoltage() > MAX_TURRET_POT) && (par.center_mass_x_normalized < 0))
					{
						turret.Set(0);
					}
					else if ((potTurret.GetVoltage() < MIN_TURRET_POT) && (par.center_mass_x_normalized > 0))
					{
						turret.Set(0);
					}
					else
					{
						pidControlTurret.Enable();
						turret.Set(-pidTurretX.x);
						pidControlTurret.SetSetpoint(-stick4.GetZ() * 0.35);
						//pidControlTurret.SetSetpoint(0);

						pidControlHood.Enable();
						hood.Set(pidHood.hood);
						pidControlHood.SetSetpoint(hood_amp * (par.particleToImagePercent - POINT_BLANK_SIZE) * (par.particleToImagePercent - POINT_BLANK_SIZE) + TRAILER_HOOD_POT);
					}
				}
				else
				{
					if (!stick4.GetRawButton(2) && !stick4.GetRawButton(1))
					{
						TurretManual();				
						HoodManual();
					}
				}

				/*
				// For the backup LED Box
				if (par.center_mass_x_normalized < -0.6)
					camLED = 5;
				else if (par.center_mass_x_normalized < -0.2)
					camLED = 4;
				else if (par.center_mass_x_normalized < 0.2)
					camLED = 3;
				else if (par.center_mass_x_normalized < 0.6)
					camLED = 2;
				else
					camLED = 1;

				if (par.particleToImagePercent > 3.70)
					RangeLED = 6;
				else if (par.particleToImagePercent > 0.2)
					RangeLED = 5;
				else if (par.particleToImagePercent > 0.1)
					RangeLED = 4;
				else if (par.particleToImagePercent > 0.05)
					RangeLED = 3;
				else
					RangeLED = 2;
				 */

				// For the primary LED Box
				if (par.center_mass_x_normalized < -0.6)
					camLED = 6;
				else if (par.center_mass_x_normalized < -0.2)
					camLED = 5;
				else if (par.center_mass_x_normalized < 0.2)
					camLED = 4;
				else if (par.center_mass_x_normalized < 0.6)
					camLED = 3;
				else
					camLED = 2;

				if (par.particleToImagePercent > 3.70)
					RangeLED = 6;
				else if (par.particleToImagePercent > 0.2)
					RangeLED = 5;
				else if (par.particleToImagePercent > 0.1)
					RangeLED = 4;
				else if (par.particleToImagePercent > 0.05)
					RangeLED = 3;
				else
					RangeLED = 2;
			} 
			else 
			{
				if (!stick4.GetRawButton(2) && !stick4.GetRawButton(1))
				{
					TurretManual();
					HoodManual();
				}
				
				camLED = 0;
				RangeLED = 0;
			}
			/*
			// For the backup LED Box
			ds->SetDigitalOut(3, camLED % 2);
			ds->SetDigitalOut(4, (camLED >> 1) % 2);
			ds->SetDigitalOut(5, (camLED >> 2) % 2);
			ds->SetDigitalOut(6, RangeLED % 2);
			ds->SetDigitalOut(7, (RangeLED >> 1) % 2);
			ds->SetDigitalOut(8, (RangeLED >> 2) % 2);
			 */

			// For the primary LED Box
			ds->SetDigitalOut(3, camLED % 2);
			ds->SetDigitalOut(4, (camLED >> 1) % 2);
			ds->SetDigitalOut(5, (camLED >> 2) % 2);
			ds->SetDigitalOut(6, RangeLED % 2);
			ds->SetDigitalOut(7, (RangeLED >> 1) % 2);
			ds->SetDigitalOut(8, (RangeLED >> 2) % 2);

			//ShowActivity("Autopot: %f, Autoswitch: %d", ds->GetAnalogIn(4), ds->GetDigitalIn(1)); 
			ShowActivity("Turret: %f, Hood: %f, Trap: %f, Shooter: %d, Size: %f", potTurret.GetVoltage(), potHood.GetVoltage(), potTrapdoor.GetVoltage(), pidShooterEncoder.shooterEncoder->Get(), par.particleToImagePercent);

		}
		else
		{
			if (!tracking)
			{
				if (!stick4.GetRawButton(2) && !stick4.GetRawButton(1))
				{
					TurretManual();
					HoodManual();
				}
			}
		}
	}
};

START_ROBOT_CLASS(RobotDemo);
